import threading
from numpy import cos, sin, tan
import numpy as np
import pathPlan
from algo import rad_limit
from myStruct import MyDynCoe, ControlParams, APFCoe
import time


# 运动仿真
def sec(angle: float) -> float:
	'''
    sec三角函数计算
    :param angle:弧度制
    :return: 返回计算结果或者报错
    '''
	try:
		return 1 / cos(angle)
	except OSError as error_msg:
		print(error_msg)
		print("angle is zero!")


def uuv_b2n(u, x):
	'''
    坐标转换函数, {B}到{N}
    :param u:状态6
    :param x:位置6
    :return:转换后的状态nu 6
    '''
	phi = x[3]
	theta = x[4]
	psi = x[5]
	trans = np.array([
		[cos(psi) * cos(theta), cos(psi) * sin(theta) * sin(phi) - sin(psi) * cos(phi),
		 cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(phi), 0, 0, 0],
		[sin(psi) * cos(theta), sin(psi) * sin(theta) * sin(phi) + cos(psi) * cos(phi),
		 sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi), 0, 0, 0],
		[-sin(theta), cos(theta) * sin(phi), cos(phi) * sec(theta), 0, 0, 0],
		[0, 0, 0, 1, sin(phi) * tan(theta), cos(phi) * tan(theta)],
		[0, 0, 0, 0, cos(phi), -sin(phi)],
		[0, 0, 0, 0, sin(phi) * sec(theta), cos(phi) * sec(theta)]
	])
	return np.dot(trans, u)


class MyMotionSim(threading.Thread):
	'''
    运动仿真线程类
    调用后生成运动轨迹并绘图

    '''

	def __init__(self, q, path, objs, boat_set, mode) -> None:
		'''

        :param q: queue.Queue()消息队列 用于和主线程通信
        :param path: 输入路径点 n*3矩阵n*[x, y, z] 离散后的不定长3维路径点
        :param objs: 障碍物 位置3 速度1 运动方向角1 长度1 宽度1 高度1 避障半径1 类型1 n*10矩阵
        :param boat_set: 仿真参数设置:[ 总的仿真时间float, 期望船速float, 初始船位置np 6*1]
        :param mode: 任务类型
        '''
		super().__init__()
		self.q = q
		self.boat = MyDynCoe()
		self.control = ControlParams()
		self.coe = APFCoe()
		self.coe.goal = path[len(path)-1, :]
		self.control.x = boat_set[2]
		# 总仿真时间
		self.t = 0
		self.h = 0.5
		self.all_time = boat_set[0]
		# 回传仿真路径
		self.points_back = []
		# 期望路径
		self.points_xy = path
		# 障碍物目标
		self.objs = objs
		# 期望速度 减速速度
		self.control.target_u = boat_set[1]
		self.control.low_u = boat_set[1]
		# 任务类型
		self.control.mode = mode

	def init_motion(self) -> None:
		# init motion status params
		self.boat = MyDynCoe()
		self.control = ControlParams()
		self.t = 0
		self.control.flag = 0
		# 实时仿真 初始步
		self.h = 0.5
		# 步长
		self.all_time = 1000

	def mmg_equ(self,
	            u,
	            x,
	            delta,
	            f):
		'''
        动力学方程
        :param u:
        :param x:
        :param delta:
        :param f:
        :return:
        '''
		if len(u) == 6 and len(x) == 6 and len(delta == 3) and len(f) == 6:
			phi = x[3]
			theta = x[4]
			psi = x[5]
			delta_r = delta[0]
			delta_s = delta[2]

			uu = u[0]
			v = u[1]
			w = u[2]
			p = u[3]
			q = u[4]
			r = u[5]

			l_2 = 0.5 * self.boat.rho_w * (self.boat.l ** 2)
			l_3 = 0.5 * self.boat.rho_w * (self.boat.l ** 3)
			l_4 = 0.5 * self.boat.rho_w * (self.boat.l ** 4)
			l_5 = 0.5 * self.boat.rho_w * (self.boat.l ** 5)

			m_matrix = np.array([
				[self.boat.m - l_3 * self.boat.x_u_, 0, 0, 0, 0, 0],
				[0, self.boat.m - l_3 * self.boat.y_v_, 0, 0, 0, -l_4 * self.boat.y_r_],
				[0, 0, self.boat.m - l_3 * self.boat.z_w_, 0, -l_4 * self.boat.z_q_, 0],
				[0, 0, 0, self.boat.ine_xx, 0, 0],
				[0, 0, -l_4 * self.boat.m_w_, 0, self.boat.ine_yy - l_5 * self.boat.m_q_, 0],
				[0, -l_4 * self.boat.n_v_, 0, 0, 0, self.boat.ine_zz - l_5 * self.boat.n_r_]
			])

			c_matrix = np.array([
				[l_2 * self.boat.x_uu * abs(uu), 0, 0, 0, self.boat.m * w, -self.boat.m * v],
				[l_3 * self.boat.y_r * r + l_3 * self.boat.y_v * v, 0, 0, -self.boat.m * w, 0, self.boat.m * uu],
				[l_3 * self.boat.z_q * q + l_3 * self.boat.z_w * w, 0, 0, self.boat.m * v, -self.boat.m * uu, 0],
				[0, l_4 * self.boat.k_vq * q, l_4 * self.boat.k_wr * r, 0, self.boat.ine_zz * r, -self.boat.ine_yy * q],
				[l_4 * self.boat.m_q * q + l_3 * self.boat.m_w * w, 0, 0, -self.boat.ine_zz * r, 0,
				 self.boat.ine_xx * p],
				[l_4 * self.boat.n_r * r + l_4 * self.boat.n_v * v, 0, 0, self.boat.ine_yy * q, -self.boat.ine_xx * p,
				 0]
			])

			g_matrix_u = np.array([
				0,
				0,
				0,
				-self.boat.m * self.boat.g_e * (self.boat.z_g - self.boat.z_b) * cos(theta) * sin(phi),
				-self.boat.m * self.boat.g_e * (self.boat.z_g - self.boat.z_b) * sin(theta),
				0
			])

			tau = f
			rudder = np.array([
				0,
				l_2 * self.boat.y_d_r * (uu ** 2) * delta_r,
				l_2 * self.boat.z_d_s * (uu ** 2) * delta_s,
				0,
				l_3 * self.boat.m_d_s * (uu ** 2) * delta_s,
				l_3 * self.boat.n_d_r * (uu ** 2) * delta_r
			])
			out = np.dot(np.linalg.inv(m_matrix), np.dot(c_matrix, u) + rudder + tau + g_matrix_u)
			# print('out', out)
			return out

	def run(self) -> None:
		'''
        线程run函数
        :return: none
        '''
		start_t = time.time()
		while self.t <= self.all_time:
			self.t += self.h
			k1 = self.mmg_equ(self.control.u, self.control.x, self.control.delta, self.control.f)
			k2 = self.mmg_equ(self.control.u + 0.5 * self.h * k1, self.control.x, self.control.delta, self.control.f)
			k3 = self.mmg_equ(self.control.u + 0.5 * self.h * k2, self.control.x, self.control.delta, self.control.f)
			k4 = self.mmg_equ(self.control.u + self.h * k3, self.control.x, self.control.delta, self.control.f)
			self.control.u = self.control.u + (self.h / 6) * (k1 + 2 * k2 + 2 * k3 + k4)

			# print('状态数据u:', self.u)
			self.control.nu = uuv_b2n(self.control.u, self.control.x)
			# print('位置数据x:', self.x)
			self.control.x = self.control.nu * self.h + self.control.x
			self.control.x[3] = rad_limit(self.control.x[3])
			self.control.x[4] = rad_limit(self.control.x[4])
			self.control.x[5] = rad_limit(self.control.x[5])
			self.points_back.append(self.control.x)
			# 任务类型: 优先路径跟踪高精度 优先动态避障（仅遍历路径点）
			if self.control.mode == 1:
				# 路径插值与跟踪
				if self.control.flag >= len(self.points_xy):
					# 绘图
					break
				else:
					# 更新f和delta，增量PID的参数, 形参输入不是引用形式，不会刷新输入的值，需要返回才行。
					self.control = pathPlan.line_tracking(self.control, self.points_xy)
			elif self.control.mode == 2:
				if self.control.flag == 1:
					break
				else:
					self.control = pathPlan.main_cv(self.control, self.objs, self.coe)
		end_time = time.time()
		print('simulation time is :', end_time - start_t)
		# 按顺序依次放入消息队列
		self.q.put(self.points_back)
		# 保存本次仿真迭代修改的参数


